/***************************************************************************
filter.c - DIGITAL FILTER CLCULATION MODULE


This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

Copyright (C) 2013  Bakhurin Sergey
***************************************************************************/


#include <stdio.h> 
#include <stdlib.h> 
#define _USE_MATH_DEFINES
#include <math.h>
#include <string.h>
#include "filter.h"
#include "specfunc.h"
#include "dsp.h"
#include "cmplx.h"



/***************************************************************************
ANALOG NORMALIZED BUTTERWORTH LOWPASS FILTER CALCULATION

Function calculates 

		  b[0] + b[1]*s + b[2]*s^2 + ... + b[ord]*s^ord
H(s) = ---------------------------------------------------
		  a[0] + a[1]*s + a[2]*s^2 + ... + a[ord]*s^ord

of analog lowpass Batterworth filter with pass frequency wp = 1 rad/sec.  


Input parameters:
	typedsp Rp	- passband distortion (dB). |H(s)| = Rp dB for s = j*1.   
	long ord	- filter order. Filter numerator and denomerator 
				  coeff count is ord+1 

Output parameters:					  
	typedsp* b	- numerator	coeff vector b[0] + b[1]*s + b[2]*s^2... 
	typedsp* a	- denomenator coeff vector a[0] + a[1]*s +a[2]*s^2 ...

Return: 
	FILTER_OK if calculation fineshed succesfully
	FILTER_ERROR if error

Note:
	Memory must be alloced before for "a" and "b" vectors. */

long butterNorm(typedsp Rp, long ord, typedsp* b, typedsp* a)
{
	typedsp ep, p[3] = {0.0, 0.0, 1.0}, alpha, teta, *acc;
	long r, L, n, k;
	
	if(Rp < 0 || Rp == 0 || ord < 1 || !b || !a)
		return FILTER_ERROR;
	
	acc = (typedsp*)malloc((ord+1)*sizeof(typedsp));

	memset(acc, 0,  (ord+1)*sizeof(typedsp));
	memset(a,   0,  (ord+1)*sizeof(typedsp));
	memset(b,   0,  (ord+1)*sizeof(typedsp));


	ep = sqrt(pow(10.0, Rp*0.1) - 1.0);
	r = ord % 2;
	L = (long)((ord-r)/2);
	
	b[0] = 1.0/ep;
	
	alpha = pow(ep, -1.0/(typedsp)ord);
	// first pole according to filter order
	if(r)
	{
		// we have one real pole if filter order is odd 
		acc[0] = alpha;
		acc[1] = 1.0;
		k = 2;
	}
	else
	{
		//all poles are complex if filter order is even  
		acc[0] = 1.0;
		k = 1;
	}
	
	// coeff calculation
	for(n = 0; n < L; n++)
	{
		teta = M_PI*(typedsp)(2*n+1)/(typedsp)(2*ord);
		p[0] = alpha * alpha;
		p[1] = 2.0 * alpha * sin(teta);
		conv(p, 3, acc, k, a);
		k+=2;
		memcpy(acc, a, k*sizeof(typedsp));
	}

	free(acc);
	return FILTER_OK;
}



/***************************************************************************
ANALOG NORMALIZED CHEBYCHEV TYPE 1 LOWPASS FILTER CALCULATION

Function calculates 

		  b[0] + b[1]*s + b[2]*s^2 + ... + b[ord]*s^ord
H(s) = ---------------------------------------------------
		  a[0] + a[1]*s + a[2]*s^2 + ... + a[ord]*s^ord

of analog lowpass Chebychev type 1 filter with pass 
frequency wp = 1 rad/sec.  


Input parameters:
	typedsp Rp	- passband distortion (dB). |H(s)| = Rp dB for s = j*1.   
	long ord	- filter order. Filter numerator and denomerator 
				  coeff count is ord+1 

Output parameters:					  
	typedsp* b	- numerator	coeff vector b[0] + b[1]*s + b[2]*s^2... 
	typedsp* a	- denomenator coeff vector a[0] + a[1]*s +a[2]*s^2 ...

Return: 
	FILTER_OK if calculation fineshed succesfully
	FILTER_ERROR if error

Note:
	Memory must be alloced before for "a" and "b" vectors. */

long cheby1Norm(typedsp Rp, long ord, typedsp *b, typedsp *a)
{
	typedsp ep, Gp,  beta, *acc, alpha, sigma, omega, p[3] = {0.0, 0.0, 1.0};
	long r, L, n, k;

	if(Rp < 0 || Rp == 0 || ord < 1 || !b || !a)
		return FILTER_ERROR;

	acc = (typedsp*)malloc((ord+1)*sizeof(typedsp));

	memset(acc, 0,  (ord+1)*sizeof(typedsp));
	memset(a,   0,  (ord+1)*sizeof(typedsp));
	memset(b,   0,  (ord+1)*sizeof(typedsp));

	ep = sqrt(pow(10.0, Rp*0.1) - 1.0);
	Gp = sqrt(1.0 / (1.0 + ep*ep));
	r = ord % 2;
	L = (long)((ord-r)/2);

	beta = asinh(1.0/ep)/(typedsp)ord;
	b[0] = Gp;
	// first pole according to filter order
	if(r)
	{
		// we have one real pole if filter order is odd 
		b[0] = acc[0] = sinh(beta);
		acc[1] = 1.0;
		k = 2;
	}
	else
	{
		// all poles are complex if filter order is even  
		acc[0] = 1.0;
		b[0] = Gp;
		k = 1;
	}

	// coeff calculation
	for(n = 0; n < L; n++)
	{
		alpha = M_PI*(typedsp)(2*n+1)/(typedsp)(2*ord);
		sigma = -sin(alpha) * sinh(beta);
		omega =  cos(alpha) * cosh(beta);

		p[0] = sigma * sigma + omega * omega;
		b[0] *= p[0];
		p[1] = -2.0 * sigma;
		conv(p, 3, acc, k, a);
		k+=2;
		memcpy(acc, a, k*sizeof(typedsp));
	}

	free(acc);
	return FILTER_OK;
}



/***************************************************************************
ANALOG NORMALIZED CHEBYCHEV TYPE 2 LOWPASS FILTER CALCULATION

Function calculates 

		  b[0] + b[1]*s + b[2]*s^2 + ... + b[ord]*s^ord
H(s) = ---------------------------------------------------
		  a[0] + a[1]*s + a[2]*s^2 + ... + a[ord]*s^ord

of analog lowpass Chebychev type 1 filter with stop 
frequency wp = 1 rad/sec.  


Input parameters:
	typedsp Rs	- stopband suppression (dB). |H(s)| = -Rs dB for s = j*1.   
	long ord	- filter order. Filter numerator and denomerator 
				  coeff count is ord+1 

Output parameters:					  
	typedsp* b	- numerator	coeff vector b[0] + b[1]*s + b[2]*s^2... 
	typedsp* a	- denomenator coeff vector a[0] + a[1]*s +a[2]*s^2 ...

Return: 
	FILTER_OK if calculation fineshed succesfully
	FILTER_ERROR if error

Note:
	Memory must be alloced before for "a" and "b" vectors. */

long cheby2Norm(typedsp Rs, long ord, typedsp *b, typedsp *a)
{
	typedsp es, *acc, *bcc, alpha, beta, sigma, omega, sh, ch, so2;
	typedsp p[3] = {0.0, 0.0, 1.0}, q[3] = {0.0, 0.0, 1.0}, gain;
	long r, L, n, kp;
	
	if(Rs < 0.0 || Rs == 0.0 || ord < 1 || !b || !a)
		return FILTER_ERROR;
	
	acc = (typedsp*)malloc((ord+1)*sizeof(typedsp));
	bcc = (typedsp*)malloc((ord+1)*sizeof(typedsp));

	memset(acc, 0,  (ord+1)*sizeof(typedsp));
	memset(bcc, 0,  (ord+1)*sizeof(typedsp));
	memset(a,   0,  (ord+1)*sizeof(typedsp));
	memset(b,   0,  (ord+1)*sizeof(typedsp));

	es = sqrt(pow(10.0, Rs*0.1) - 1.0);
	r = ord % 2;
	L = (long)((ord-r)/2);

	beta = asinh(es)/(typedsp)ord;

	sh = sinh(beta);
	ch = cosh(beta);

	bcc[0] = 1.0;
	// first pole according to filter order
	if(r)
	{
		// we have one real pole if filter order is odd 
		acc[0] = 1.0/sh;
		acc[1] = 1.0;
		kp = 2;
	}
	else
	{
		// all poles are complex if filter order is even  
		acc[0] = 1.0;
		kp = 1;
	}

	// coeff calculation
	for(n = 0; n < L; n++)
	{
		alpha = M_PI*(typedsp)(2*n+1)/(typedsp)(2*ord);
		
		sigma = sh*sin(alpha);
		omega = ch*cos(alpha);

		so2 = 1.0 / (sigma*sigma+omega*omega);

		q[0] = 1.0/(cos(alpha)*cos(alpha));		
		p[0] = so2;
		p[1] = 2.0*sigma*so2;

		conv(p, 3, acc, kp, a);
		conv(q, 3, bcc, kp, b);
		kp+=2;
		memcpy(acc, a,  kp * sizeof(typedsp));
		memcpy(bcc, b,  kp * sizeof(typedsp));
	}

	gain = b[0] / a[0];
	for(n = 0; n < ord+1; n++)
		b[n] /= gain;

	free(acc);
	free(bcc);
	
	return FILTER_OK;
}

	


/***************************************************************************
REAL CONVOLUTION

Input parameters:
	typedsp* a	- pointer to first real vector 
	long na		- vector "a" length
	typedsp* b	- pointer to second real vector
	long nb		- vector "b" length

Output parameters:
	typedsp* c	- pointer to result. Size of vector "c" is na+nb-1. 

Return: 
	FILTER_OK if calculation fineshed succesfully
	FILTER_ERROR if error

Note:
	Memory must be alloced before for "c" vector. */

long conv(typedsp* a, long na, typedsp* b, long nb, typedsp* c)
{
	long n,k;
	if(!a || !b || !c || na < 1 || nb < 1 || c == a || c == b)
		return FILTER_ERROR;
	memset(c, 0, (na+nb-1)*sizeof(typedsp));
	for(n = 0; n < na; n++)
		for(k = 0; k < nb; k++)
			c[n+k] += a[n] * b[k];
	return FILTER_OK;
}



/***************************************************************************
COMPLEX CONVOLUTION

Input parameters:
	complex* a	- pointer to first complex vector 
	long na		- vector "a" length
	complex* b	- pointer to second complex vector
	long nb		- vector "b" length

Output parameters:
	complex* c	- pointer to result. Size of vector "c" is na+nb-1. 

Return: 
	FILTER_OK if calculation fineshed succesfully
	FILTER_ERROR if error

Note:
	Memory must be alloced before for "c" vector. */

long convCmplx(complex* a, long na, complex* b, long nb, complex* c)
{
	long n,k;
	if(!a || !b || !c || na < 1 || nb < 1 || c == a || c == b)
		return FILTER_ERROR;
	memset(c, 0, (na+nb-1)*sizeof(complex));
	for(n = 0; n < na; n++)
		for(k = 0; k < nb; k++)
			c[n+k] = sumCmplx(c[n+k], mulCmplx(a[n], b[k])); 
	return FILTER_OK;
}




/***************************************************************************
ANALOG FILTER TRANSFER FUNCTION H(s) CALCULATION

Input parameters:
	complex *b	- pointer to vector of H(s) numerator coefficients 
	long nb		- vector "b" length
	complex *a	- pointer to vector of H(s) denomintor coefficients
	long na		- vector "a" length
	typedsp *w	- ponter to frequecy vector in rad/sec
	long nw		- frequency vector length
Output parameters:
	complex* h	- pointer to result. Size of vector "h" is nw. 
				  Each h[k] = H(j*w[k]), here j = sqrt(-1).
Return: 
	FILTER_OK if calculation fineshed succesfully
	FILTER_ERROR if error

Note:
	Memory must be alloced before for "h" vector. */

long freqs(typedsp* b, long nb, typedsp* a, long na, typedsp* w, long nw, complex* h)
{
	complex jw;
	long n;

	if(!b || !a || !w || !h || nb<1 || na<1 || nw<1)
		return FILTER_ERROR;

	for(n = 0; n<nw; n++)
	{
		jw = numCmplx(0.0, w[n]); // s[k] = j*w[k]
		
		// H(jw) calculation
		h[n] = divCmplx( polyValCmplx(b, nb, jw), polyValCmplx(a, na, jw) );
	}

	return FILTER_OK;
}